#include "AS5600.h"
#include "i2c.h"

#define PI 3.1415926535f

volatile float last_angle = 0.0f;
volatile float this_angle = 0.0f;
volatile float full_rotations = 0.0f;// 圈数
volatile float last_full_rotations = 0.0f;//上一次的圈数

volatile float now_speed = 0.0f;
volatile float real_speed = 0.0f;
volatile float last_speed = 0.0f;
volatile float sum_angle = 0.0f;
float Get_AS5600_Angle(void)
{
    float jiaodu = 0;
    float hudu = 0;
    uint8_t IIC_buffer[2] = {0};
    HAL_I2C_Mem_Read(&hi2c1, 0x6c, 0x0c, I2C_MEMADD_SIZE_8BIT, IIC_buffer, 2, 100);

    jiaodu = (IIC_buffer[0] * 256 + IIC_buffer[1])*0.087890625;
    hudu = (IIC_buffer[0] * 256 + IIC_buffer[1])*(2.0f*PI) / 4096;

    // return jiaodu;
    return hudu;
}

float Get_Sum_Angle()
{
    float D_angle = 0.0f;
    D_angle = this_angle - last_angle;
    if (D_angle > 5.2f)
    {
        full_rotations -= 1;
    }
    if (D_angle < -5.2f)
    {
        full_rotations += 1;
    }
    // last_angle = this_angle;
    return (full_rotations * 2 * PI + this_angle);
}

void UpDate_Data() // 更新角度信息
{
    this_angle = Get_AS5600_Angle();
    sum_angle = Get_Sum_Angle();
    now_speed = Get_Speed();
    real_speed = Low_Pass_Filter(now_speed);

    //最后才更新数据到上一次
    last_angle = this_angle;
    last_full_rotations = full_rotations; // 更新 last_full_rotations
}

float Get_Speed()
{
    float speed = (((float)(full_rotations - last_full_rotations) * 2 * PI) + (this_angle - last_angle)) / 1e-3f;
    return speed;
}


// 低通滤波器
float Low_Pass_Filter(float speed)
{
    float alpha = 0.9f;

    float output_speed = speed * (1.0f - alpha) + last_speed * alpha; 
    // output = Constrain(output, -30.0f, 30.0f);
    // if(output > 20 || output < -20) output = last_speed;
    // else last_speed = output;
    last_speed = output_speed;

    return output_speed;
}